#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  rightBumper,    sensorTouch)
#pragma config(Sensor, dgtl6,  leftBumper,     sensorTouch)
#pragma config(Sensor, dgtl7,  sonar,          sensorSONAR_cm)
#pragma config(Motor,  port2,           rightMotor,    tmotorVex269, openLoop)
#pragma config(Motor,  port3,           leftMotor,     tmotorVex269, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void forward(int revs){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  int turns=revs*360;
  while(SensorValue[rightEncoder]>-turns){
    if(SensorValue[rightEncoder]>SensorValue[leftEncoder]){
      motor[rightMotor]=45;
      motor[leftMotor]=50;
    }
    else if(SensorValue[leftEncoder]>SensorValue[rightEncoder]){
      motor[leftMotor]=45;
      motor[rightMotor]=50;
      }
      else if(SensorValue[rightEncoder]==SensorValue[leftEncoder]){
        motor[rightMotor]=45;
        motor[leftMotor]=50;
      }
    }
    }
void turnright(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(-SensorValue[leftEncoder]<45){
    motor[rightMotor]=-50;
    motor[leftMotor]=40;
  }
}
void slightRight(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(-SensorValue[rightEncoder]<-180){
    motor[rightMotor]=-50;
    motor[leftMotor]=-50;
  }
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(-SensorValue[leftEncoder]<80){
    motor[rightMotor]=-80;
    motor[leftMotor]=80;
}
}
void turnleft(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(-SensorValue[rightEncoder]>180){
    motor[rightMotor]=50;
    motor[leftMotor]=-50;
  }
}
void slightLeft(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(SensorValue[rightEncoder]<180){
    motor[rightMotor]=-60;
    motor[leftMotor]=-60;
  }
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(-SensorValue[rightEncoder]<80){
    motor[rightMotor]=80;
    motor[leftMotor]=-80;
  }
}

void checkDirection(){
       int lCheck;
       int rCheck;
       turnleft();
       lCheck=SensorValue[sonar];
       turnright();
       turnright();
       rCheck=SensorValue[sonar];
       turnleft();
   if(SensorValue[rightBumper]>SensorValue[leftBumper]){
         motor[rightMotor]=-40;
         motor[leftMotor]=40;
       }
       else if(SensorValue[rightBumper]<SensorValue[leftBumper]){
         motor[rightMotor]=40;
         motor[leftMotor]=-40;
     }
}
task main(){
  while(1==1){
    while(SensorValue[sonar]>40||SensorValue[sonar]<1){
  if(SensorValue[leftBumper]==0 && SensorValue[rightBumper]==0){
  forward(1);
}
  else if(SensorValue[leftBumper]==1){
  slightRight();
}
  else if(SensorValue[rightBumper]==1){
  slightLeft();
  }
 }
 checkDirection();
  }
}
